;;;; 2010-12-18 22:54:35
;;;; This is your lisp file. There are many like it, but this one is yours.

(in-package :partial-order-planner)

(defun poplanner  (initial goal ops) 
  (let* ((plan (make-minimal-plan initial goal))
         (problem (make-problem :init-state plan
                                :operators ops
                                :goal-test #'solution?
                                :path-cost-fn (strategy-selector 'as1)))
         (node nil))
    (setf node (first (uniform-cost-search problem)))
    (if node (node-state node))
    )
  )
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;Searching;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun general-search (problem qing-fun &optional (cut-off nil cut-off-supplied-p))
  
  (let* ((nodes (list (make-node :state (problem-init-state problem) 
                                 :depth 0 
                                 :path-cost 0 
                                 :parent nil 
                                 :operator nil ) ()))
         (num-expanded (list-length nodes)))
    (loop do 
          (if   (eq 0 (list-length nodes))
              (return (list nil num-expanded))
              )
          (let ((node nil)
                (expanded-nodes nil)) 
            (setf node (first nodes))
            (if (equal node nil)
                (return (list nil num-expanded))
                )
            ;(print (funcall (problem-goal-test problem) (node-state node)))
            (if (funcall (problem-goal-test problem) (node-state node))
                (return (list node num-expanded))
                )
            
            ;(setf expanded-nodes nil)
            ;(print (problem-operators problem))
            (if (eq t cut-off-supplied-p)
                (if (> cut-off (node-depth node))
                    (setf expanded-nodes (expand-node node problem))
                    )
                (setf expanded-nodes (expand-node node problem))
                )
            
            (setf num-Expanded (+ num-expanded (list-length expanded-nodes)))
            (setf nodes (funcall qing-fun (rest nodes) expanded-nodes)))
          )
    )
  )

(defun uniform-cost-search (problem &optional (cut-off nil cut-off-supplied-p))
  (if (eql t cut-off-supplied-p)
      (general-search problem #'ordered-insert cut-off)
      (general-search problem #'ordered-insert)
      )
  )

(defun ordered-insert (nodes exp-node-list)
  (let ((non-ordered nil))
    (if (equal nodes nil)
        (setf non-ordered exp-node-list)
        (if (equal exp-node-list nil)
            (setf non-ordered nodes)
            (setf non-ordered (append nodes exp-node-list))
            )
        )
    (if (not  (equal non-ordered nil))
        (sort (remove nil non-ordered) #'< :key #'node-path-cost)
        )
    )
  )

(defun expand-node (node problem)
  (let ((exp-nodes ())
        (depth (node-depth node))
        (av-op (av-ops (node-state node) (problem-operators problem))))
    (dolist (i av-op)
      (let ((new-node (make-node :state  (apply-op (node-state node) i) 
                                 :operator i
                                 :parent node
                                 :depth (1+ depth) 
                                 )
                      ))
        (setf (node-path-cost new-node) (funcall (problem-path-cost-fn  problem) node problem))
        ;(add-state (node-state new-node))
        (setf exp-nodes (cons new-node exp-nodes)))
      )
    exp-nodes)
  )

(defun  strategy-selector (strategy)
  (let ((fun nil))
    (cond 
      ((eq strategy 'bf)(setf fun (lambda (node problem) (node-depth node))))
      ;((eq strategy 'df)(setf fun (lambda (node) (* -1 (node-depth node)))))
      ;((eq strategy 'it)(setf fun (lambda (node) (* -1 (node-depth node)))))
      ;((eq strategy 'gr1)(setf  fun #' Manhattan))
      ;((eq strategy 'gr2)(setf fun #'misplaced-tiles-h))
      ;((eq strategy 'gr3)(setf fun #'blank))
      ((eq strategy 'as1)(setf fun (lambda (node problem) (+ (node-depth node) (funcall #'search-heuristic node problem)))))
      ;((eq strategy 'as2) (setf fun (lambda (node goal-state) (+ (node-depth node) (funcall #'misplaced-tiles-h node goal-state)))))
      ;((eq strategy 'as3)(setf fun (lambda (node goal-state) (+ (node-depth node) (funcall #'blank node goal-state)))))
      (t nil)
      )
    fun
    )
  )
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;Unification;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; Created on 2010-12-20 00:08:48
(defun unify (x y &optional subst)
  "Tries unifies two expressions x and y returning the substitution list that unifies them"
  (cond ((equal x y) subst)
        ((equal subst 'fail) 'fail)
        ((var? x) (unify-variable x y subst))
        ((var? y) (unify-variable y x subst))
        ((or (atom x) (atom y)) 'fail)
        (t (unify (rest x) (rest y) (unify (first x) (first y) subst)))
   )
 )

(defun unify-variable (var val subst)
"Unify var with val, using (and possibly extending) subst."
	(cond ((equal var val) subst)
		  ((bound? var subst)(unify (lookup var subst) val subst))
			;; New condition: dereference val when it is a variable
		  ((and (var? val) (bound? val subst))(unify var (lookup val subst) subst))
		  ((occurs-in? var val subst) 'fail)
		  (t (extend-subst var  val subst))
    )
 )
(defun occurs-in? (var x subst)
"Does var occur anywhere inside x?"
  (cond ((equal var x) t)
    ((bound? x subst) (occurs-in? var (lookup x subst) subst))
    ((consp x) (or (occurs-in? var (first x) subst)(occurs-in? var (rest x) subst)))
    (t nil)
    )
  )
(defun var? (x) "Is x a variable?" (member x '(U V X Y Z)))
(defun bound? (x subst) "Is x a bound variable?" (assoc x subst))
(defun lookup (var subst) (cdr (cdr (assoc var subst))))
(defun replace-any-where-rec1 (var val cons-list)
(if (consp (cdr cons-list))
  (if (member var (cdr cons-list))
    (rplaca (member var cons-list) val)
    (if (rest cons-list)
		(replace-any-where-rec1 var val (rest cons-list))
    )
  )
  (if (equal var (cdr cons-list))
	(setf (cdr cons-list) val)
  )
  )
 )
(defun replace-any-where-rec2 (var val cons-list)
	(if (consp val)
		(if (member (car cons-list) val)
			(rplaca (member (car cons-list) val)(cdr (cdr cons-list)))
			(replace-any-where-rec2 var (rest val) cons-list)
		)
		(if (equal val(car cons-list))
			(setf val (cdr (cdr cons-list)))
		)
	)
 )
(defun replace-any-where (var val subst)
  (dotimes (i (length subst))
    (replace-any-where-rec1 var val (nth i subst))
	(replace-any-where-rec2 var val (nth i subst))
    )
  )
(defun extend-subst (var val subst) 
  (replace-any-where var val subst)
  (if (consp val)
  (cons (cons var (cons '/ val )) subst)
  (cons (cons var (cons '/ (cons val ()))) subst)
  )
 )
(defun apply-subst (term subst)
  (if (consp term)
      (dotimes (i (length term))
        (setf (nth i term) (apply-subst (nth i term) subst))
        )
      (let ((val (car (lookup term subst))))
        (if val
            (setf term val)
            )
        term
        )
      )
  term
  )
(defun apply-subst-helper (term subst)
  (if (consp term)
      (dotimes (i (length term))
        (setf (nth i term) (apply-subst-helper (nth i term) subst))
        )
      (let ((val (lookup term subst)))
        (if val
            (setf term val)
            )
        term
        )
      )
  )

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;ADts;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;; Created on 2010-12-19 14:22:45
;;;;;;;; Search structures
(defstruct problem
  "This is the  problem struct to define a problem 
  :operators are the operators
  :init-state the initial state of the problem
  :goal-test the goal to test the node against
  :path-cost-fn the fuction that specifies the cost to each node"
  operators
  init-state
  goal-test
  path-cost-fn
  )
(defstruct node
  "a search tree node"
  path-cost
  parent
  operator 
  depth
  state
  )
;;;;;;;; Plan Structures
(defstruct plan
  "The plan Abstract data type"
  start-step
  finish-step
  steps
  causal-links
  orderings
  consistent
  )
(defstruct action
  "The action Abstract data type
   actions are intermediate steps in the plan"
  name
  preconditions
  effects
  )
(defstruct start-step
  "The start step of the plan"
  effects
  )
(defstruct finish-step
  "The Fisnish step of the plan"
  preconditions
  )
(defstruct causal-link
  "A causal link in the plan"
  cause-step
  effect-step
  sat-pre-cond
  )
(defun make-minimal-plan (start-effects finish-preconditions)
  "constructs a plan that has only the start and finish steps"
  (let ((plan (make-plan :start-step (make-start-step :effects start-effects) :finish-step (make-finish-step :preconditions finish-preconditions))))
    (setf (plan-steps plan) (cons (plan-start-step plan) (cons (plan-finish-step plan) ())))
    (setf (plan-consistent plan) t)
    plan)
  )
(defun solution? (plan)
  "Is plan a solution? The answer is 't' if there are no open preconditions in plan's steps, 'nil' otherwise"
  (if (and (plan-p plan) (plan-consistent plan))
      (equal 0 (length (get-open-conditions plan)))
      ) 
  )
(defun copy-ops (ops)
  "returns a copy of operators list"
  (let ((new-ops nil)
        (new-prec nil)
        (new-eff nil)
        (new-name nil))
    (dolist (i ops)
      (setf new-prec nil)
      (setf new-eff nil)
      (dolist (j (action-preconditions i)) (setf new-prec (reverse  (cons (copy-list j) (reverse new-prec)))))
      (dolist (j (action-effects i)) (setf new-eff (reverse (cons (if (equal 'not (car j)) (cons 'not (cons (copy-list (cadr j)) ())) (copy-list j)) (reverse new-eff)))))
      (setf new-name (action-name i))
      (setf new-ops (reverse (cons (make-action :name new-name :preconditions new-prec :effects new-eff) (reverse new-ops))))
      )
    new-ops
    )
  )
(defun  copy-plan-a (plan)
  "returns acopy of the plan"
  (if (plan-p plan)
      (let ((new-start-step (copy-start-step (plan-start-step plan)))
            (new-fin-step (copy-finish-step (plan-finish-step plan)))
            (new-caus-links nil)
            (new-steps nil)
            (new-orderings nil))
        (dolist (i (plan-steps plan))
          (setf new-steps (reverse (cons i (reverse new-steps)))))	
        (dolist (i (plan-causal-links plan))
          (setf new-caus-links (reverse (cons (copy-causal-link i) (reverse new-caus-links))))
          )
        (dolist (i (plan-orderings plan))
          (let ((first-el nil)
                (last-el nil))
             (if (consp (car i))
              (setf first-el (cons (copy-start-step (caar i)) (copy-finish-step (cdar i))))
              (if (start-step-p (car i))
                  (setf first-el (copy-start-step (car i)))
                  (setf first-el (car (copy-ops (cons (car i) ()))))
                  )
              )
            (if (action-p (cdr i))
                (setf last-el (car (copy-ops (cons (cdr i) ()))))
                (setf last-el (copy-finish-step (cdr i)))
                )
            (setf new-orderings (reverse (cons (cons first-el last-el) (reverse new-orderings))))
            )
          )
        (make-plan :start-step new-start-step :finish-step new-fin-step :steps new-steps :causal-links new-caus-links :orderings new-orderings :consistent (plan-consistent plan))
        )
      )
  )
(defun av-ops (state operators-original)
  "returns available operators (links) to add to the plan or nil if no operators exist"
  (if (and (plan-p state) (plan-consistent state))
      (progn
        (let ((steps (plan-steps state))
              (ops nil)
              (operators (copy-ops operators-original)))
          (dolist (i steps)
            (let ((pre-conds nil))
              (cond 
                ((finish-step-p i) (setf pre-conds (finish-step-preconditions i)))
                ((action-p i) (setf pre-conds (action-preconditions i)))
                (t (setf pre-conds nil))
                )
              (dolist (j pre-conds)
                (if (not (find-if #'(lambda (l)(and (eql i (causal-link-cause-step l)) (not (equal 'fail (unify j (causal-link-sat-pre-cond l)))))) (plan-causal-links state)))
                    (progn
                      (let ((flag nil)) 
                        (dolist (p (plan-steps state))
                          (if (not (eql p i))
                              (let ((effs nil)
                                    (subs nil))
                                (cond
                                  ((start-step-p p) (setf effs (start-step-effects p)))
                                  ((action-p p)(setf effs (action-effects p)))
                                  (t nil)
                                  )
                                (dolist (q effs)
                                  (setf subs (unify q j))
                                  (if (not (equal 'fail subs))
                                      (progn
                                        (setf ops (cons (cons (cons p i) (cons (copy-list j) ())) ops))
                                        (setf flag t)
                                        )
                                      )
                                  )
                                )
                              )
                          )
                        (if (not flag)
                            (dolist (k operators)
                              (let ((effs (action-effects k))
                                    (subs nil))
                                (dolist (l effs)
                                  (setf subs (unify l j))
                                  (if (not (equal 'fail subs))
                                      (setf ops (cons (cons (cons (car (copy-ops (cons k ()))) i) (cons j ())) ops)))
                                  )
                                )
                              )
                            )
                        )
                      )
                    )
                )	
              )
            )
          ops
          )
        )
      )
  )
(defun apply-op (state-original op)
  "applies op to a copy of state-original and returns the copy"
  (if (and (plan-p state-original) (consp op))
      (let*
        ((state (copy-plan-a state-original))
         (unif-eff (find-if #'(lambda (e) (not (equal 'fail (unify e (cadr op))))) (if (action-p (caar op)) (action-effects (caar op)) (start-step-effects (caar op)))))
         (subst (unify unif-eff (cadr op)))
         (eff-step-pre (if (action-p (caar op)) (action-preconditions (caar op))))
         (eff-step-eff (if (action-p (caar op)) (action-effects (caar op)) (start-step-effects (caar op))))
         (cuz-step-pre (if (action-p (cdar op)) (action-preconditions (cdar op)) (finish-step-preconditions (cdar op))))
         (cuz-step-eff (if (action-p (cdar op)) (action-effects (cdar op)))))
        ;TO-DO apply unification substitution to the effects and pre-conditions of the cause state
        (if eff-step-pre (setf (action-preconditions (caar op)) (mapcar #'(lambda (e)(apply-subst e subst)) eff-step-pre)))
        (if eff-step-eff (if (action-p (caar op)) (setf (action-effects (caar op)) (mapcar #'(lambda (e)(apply-subst e subst)) eff-step-eff))))
        (if cuz-step-pre (if (action-p (cdar op)) (setf (action-preconditions (cdar op)) (mapcar #'(lambda (e)(apply-subst e subst)) cuz-step-pre))))
        (if cuz-step-eff (setf (action-effects (cdar op)) (mapcar #'(lambda (e)(apply-subst e subst)) cuz-step-eff)))
        
        (setf (plan-causal-links state) (cons (make-causal-link :cause-step (cdar op)
                                                                :effect-step (caar op)
                                                                :sat-pre-cond (cadr op))
                                              (plan-causal-links state)))	
        (setf (plan-orderings state) (cons (car op) (plan-orderings state)))
        (if (not (find (caar op) (plan-steps state)))
            (progn
              (setf (plan-steps state) (cons (caar op) (plan-steps state)))
              (setf (plan-orderings state) (cons (cons (cons (plan-start-step state) (plan-finish-step state)) (caar op)) (plan-orderings state))))
            )
        ;;;;TO-DO Find and handle threats
        (let ((threats (get-threats state)))
          (dolist(i threats)  
            (if (can-demote? state (causal-link-effect-step (cdr i)) (car i))
                (setf (plan-orderings state) (cons (cons (car i) (causal-link-effect-step (cdr i))) (plan-orderings state)))
                (if (can-promote? state (causal-link-cause-step (cdr i)) (car i))
                    (setf (plan-orderings state) (cons (cons (causal-link-cause-step (cdr i)) (car i)) (plan-orderings state)))
                    (progn
                      (setf (plan-consistent state) nil)
                      (return state))
                    )
                )
            )
          )
        state
        )
      )
  )
(defun get-threats (plan)
  "finds the threats in plan returns the cloberring step and the threatened link"
  (if (plan-p plan)
      (let ((threats nil)
            (threat-links nil))
        (dolist (i (plan-steps plan))
          (if (action-p i)
              (progn
                (setf threat-links 
                      (remove nil 
                              (mapcar #'(lambda (e)
                                          (if (eql 'not (car e))
                                              (let ((t-link (find-if #'(lambda(l) (and (equal (causal-link-sat-pre-cond l) (cadr e))
                                                                                       (not (equal i (causal-link-cause-step l)))
                                                                                       (not (or (find (cons i (causal-link-effect-step l)) (plan-orderings plan) :test #'equal)
                                                                                                (find (cons (causal-link-cause-step l) i) (plan-orderings plan) :test #'equal))))) 
                                                                     (plan-causal-links plan) )))
                                                t-link)))
                                      (copy-list (action-effects i)))))
                (dolist (j threat-links)
                  (setf threats (cons (cons i j) threats)))
                )
              )
          )
        threats
        )
      )
  )
(defun can-demote? (plan effect-step s-threat)
  (cond
    ((start-step-p effect-step)nil)
    ((find (cons effect-step s-threat) (plan-orderings plan) :test #'eql)nil)
    (t t)
    )
  )
(defun can-promote? (plan cause-step s-threat)
  (cond
    ((finish-step-p cause-step)nil)
    ((find (cons s-threat cause-step) (plan-orderings plan) :test #'eql) nil)
    (t t))
  )
(defun get-open-conditions (plan)
  "returns the open conditions in plan"
  (let ((op-cond nil))
    (if (plan-p plan)
        (dolist (i (plan-steps plan))
          (let ((pre-conds (cond ((action-p i) (action-preconditions i)) ((finish-step-p i) (finish-step-preconditions i)))))
            (dolist (j pre-conds)
              (if (not (find j (plan-causal-links plan) :key #'causal-link-sat-pre-cond :test #'equal))
                  (setf op-cond (cons j op-cond))
                  )
              )
            )
          )
        )
    op-cond
    )
  ) 
(defun search-heuristic (node problem)
    (if (and (node-p node) (problem-p problem))
      (length (get-open-conditions (node-state node)))
      #|(let* ((plan  (node-state node))
             (ops (copy-ops (problem-operators problem)))
             (open-conds (get-open-conditions plan))
             (min-op nil))
        (dolist (i ops)
          (setf (action-effects i) (remove-if #'(lambda(e)
                                                  (eql 'not (car e)))  
                                              (action-effects i)))
          )
        (loop do
              (print open-conds)
              (if (eql 0 (length open-conds))
                  (progn
                    ;(print (cl-user::cusp-watch (length min-op)))
                    (return (length min-op)))
                  )
              (let ((max-sat-precond nil)
                    (curr-sat-precond-n nil)
                    (curr-sat-precond nil)
                    (max-act nil))
                (dolist (i ops)
                  ;(print "hi")
                  (setf curr-sat-precond-n (mapcar #'(lambda (e) (mapcar #'(lambda (p) (if (not (eql 'fail (unify e p))) p)) open-conds)) (action-effects i)))
                  ;(print "end hi")
                  (dolist(j curr-sat-precond-n)
                    (setf curr-sat-precond (append j curr-sat-precond))
                    )
                  (setf curr-sat-precond (remove nil curr-sat-precond))
                  (if (> (length curr-sat-precond) (length max-sat-precond))
                      (progn
                        (setf max-sat-precond curr-sat-precond)
                        (setf max-act i)
                        )
                      )
                  )
                (setf min-op (cons max-act min-op))
                (dolist (k max-sat-precond)
                  (setf open-conds (remove k open-conds))
                  )
                (setf ops (remove max-act ops))
                open-conds
                )
              )
        (length min-op) 
        )|#
      ;(print "not sat")
      )
  )